#include "Common.cuh"
#include<vector>
#include<filesystem>
#include <iostream>

namespace fs = std::filesystem;


#pragma region  通用功能函数实现

/// <summary>
/// GridTIFF编码转格网编码
/// </summary>
__device__ __host__ bool  gridTiffCode2GridCode(DGGSGridTIFFCode gridTiffCode, DGGSElementCode& elementCode, int originLevel)
{
    long long row = 0;
    long long col = 0;
    int level = gridTiffCode.level;
    long long morton = gridTiffCode.morton;

    for (int i = 0; i < level - originLevel; ++i) {
        row |= (morton & (1LL << (2 * i + 1))) >> (i + 1);
        col |= (morton & (1LL << (2 * i))) >> i;
    }

    row = row * (long long)pow(2, originLevel);
    col = col * (long long)pow(2, originLevel);

    elementCode.level = level;
    elementCode.basePartition = gridTiffCode.basePartition;
    elementCode.row = row;
    elementCode.col = col;
    return true;
}

/// <summary>
/// 格网编码转GridTIFF编码
/// </summary>
__device__ __host__ bool gridCode2GridTiffCode(DGGSElementCode elementCode, DGGSGridTIFFCode& gridTiffCode, int originLevel)
{
    long long row = elementCode.row;
    long long col = elementCode.col;
    int level = elementCode.level;

    long long morton = 0;
    row = row / (long long) pow(2,originLevel);
    col = col / (long long) pow(2,originLevel);
    for (int i = 0; i < level - originLevel; ++i) {
        morton |= (col & (1LL << i)) << i | (row & (1LL << i)) << (i + 1);
    }

    gridTiffCode.basePartition = elementCode.basePartition;
    gridTiffCode.morton = morton;
    gridTiffCode.level = level;

    return true;
}

/// <summary>
/// 获取边界
/// </summary>
bool getBoundFromGeoTrans(const double*geoTransform,int xSize,int ySize,double* bound)
{
    if(geoTransform== nullptr)
        return false;
    bound[2]=geoTransform[0];
    bound[3]=geoTransform[0]+xSize*geoTransform[1];
    bound[0]=geoTransform[3];
    bound[1]=geoTransform[3]+ySize*geoTransform[5];
    return true;
}

/// <summary>
/// 获取文件夹下所有文件
/// </summary>
void getAllFilesFromPath(const std::string& folderPath,const char* exFormat, std::string*& files, int& fileCount)
{
    // 用于保存所有文件名
    std::vector<std::string> matchedFiles;

    try {
        // 遍历文件夹内容
        for (const auto& entry : fs::directory_iterator(folderPath)) {
            if (fs::is_regular_file(entry.status())) { // 只获取文件，不获取目录
                std::string filePath = entry.path().string();

                // 获取文件扩展名并转化为小写
                std::string extension = entry.path().extension().string();
                if (extension == exFormat) {
                    matchedFiles.push_back(filePath); // 存储文件路径
                }
            }
        }

        // 将匹配的文件数量赋值给 tileFileCount
        fileCount =(int) matchedFiles.size();

        // 动态分配数组以保存文件路径
        files = new std::string[fileCount];

        // 将文件路径从 matchedFiles 复制到 tileFiles
        for (int i = 0; i < fileCount; ++i) {
            files[i] = matchedFiles[i];
        }
    }
    catch (const std::exception& e) {
        std::cerr << "Error accessing directory: " << e.what() << std::endl;
        fileCount = 0;  // 出现异常时，文件数量设为 0
        files = nullptr;  // 不分配内存
    }
}

/// <summary>
/// 获取文件夹下所有文件夹
/// </summary>
void getAllDirectoriesFromPath(const std::string& folderPath,std::string*& fileDirectories, int& fileDirectoriesCount) {
    std::vector<std::string> directories;

    try {
        for (const auto& entry : fs::directory_iterator(folderPath)) {
            if (entry.is_directory()) {
                std::string  currentDir=folderPath+"/"+entry.path().filename().string();
                directories.push_back(currentDir);
            }
        }
        // 将匹配的文件数量赋值给 tileFileCount
        fileDirectoriesCount =(int) directories.size();

        // 动态分配数组以保存文件路径
        fileDirectories = new std::string[fileDirectoriesCount];

        // 将文件路径从 matchedFiles 复制到 tileFiles
        for (int i = 0; i < fileDirectoriesCount; ++i) {
            fileDirectories[i] = directories[i];
        }

    } catch (const fs::filesystem_error& e) {
        std::cerr << "Error accessing folder: " << e.what() << std::endl;
    }
}


/// <summary>
/// 将格网编码转化为GridTIFF的行列号
/// </summary>
/// <param name="code"></param>
/// <param name="x"></param>
/// <param name="y"></param>
/// <param name="gridBox"></param>
/// <returns></returns>
__host__ __device__ bool code2XY(int row, int col, int rowMin, int colMin, int tileSize, int& x, int& y)
{
    int x0 = 0;
    int y0 = tileSize - 1;

    x = int(row - rowMin + col - colMin + x0);
    y = int((col - colMin) - (row - rowMin) + y0);
    if (y < 0 || x < 0)
    {
        //表示没找到，可以跳过
        x = -1, y = -1;
        return false;
    }
    return true;
}


#pragma endregion

#pragma region math

__device__  int getMatrixInverse(double* tagMatrix, double* resultMatrix)
{
    double ma[16] = {
            +tagMatrix[5] * tagMatrix[10] * tagMatrix[15] - tagMatrix[5] * tagMatrix[11] * tagMatrix[14] - tagMatrix[6] * tagMatrix[9] * tagMatrix[15] + tagMatrix[6] * tagMatrix[11] * tagMatrix[13] + tagMatrix[7] * tagMatrix[9] * tagMatrix[14] - tagMatrix[7] * tagMatrix[10] * tagMatrix[13],
            -tagMatrix[1] * tagMatrix[10] * tagMatrix[15] + tagMatrix[1] * tagMatrix[11] * tagMatrix[14] + tagMatrix[2] * tagMatrix[9] * tagMatrix[15] - tagMatrix[2] * tagMatrix[11] * tagMatrix[13] - tagMatrix[3] * tagMatrix[9] * tagMatrix[14] + tagMatrix[3] * tagMatrix[10] * tagMatrix[13],

            +tagMatrix[1] * tagMatrix[6] * tagMatrix[15] - tagMatrix[1] * tagMatrix[7] * tagMatrix[14] - tagMatrix[2] * tagMatrix[5] * tagMatrix[15] + tagMatrix[2] * tagMatrix[7] * tagMatrix[13] + tagMatrix[3] * tagMatrix[5] * tagMatrix[14] - tagMatrix[3] * tagMatrix[6] * tagMatrix[13],
            -tagMatrix[1] * tagMatrix[6] * tagMatrix[11] + tagMatrix[1] * tagMatrix[7] * tagMatrix[10] + tagMatrix[2] * tagMatrix[5] * tagMatrix[11] - tagMatrix[2] * tagMatrix[7] * tagMatrix[9] - tagMatrix[3] * tagMatrix[5] * tagMatrix[10] + tagMatrix[3] * tagMatrix[6] * tagMatrix[9],

            -tagMatrix[4] * tagMatrix[10] * tagMatrix[15] + tagMatrix[4] * tagMatrix[11] * tagMatrix[14] + tagMatrix[6] * tagMatrix[8] * tagMatrix[15] - tagMatrix[6] * tagMatrix[11] * tagMatrix[12] - tagMatrix[7] * tagMatrix[8] * tagMatrix[14] + tagMatrix[7] * tagMatrix[10] * tagMatrix[12],
            +tagMatrix[0] * tagMatrix[10] * tagMatrix[15] - tagMatrix[0] * tagMatrix[11] * tagMatrix[14] - tagMatrix[2] * tagMatrix[8] * tagMatrix[15] + tagMatrix[2] * tagMatrix[11] * tagMatrix[12] + tagMatrix[3] * tagMatrix[8] * tagMatrix[14] - tagMatrix[3] * tagMatrix[10] * tagMatrix[12],

            -tagMatrix[0] * tagMatrix[6] * tagMatrix[15] + tagMatrix[0] * tagMatrix[7] * tagMatrix[14] + tagMatrix[2] * tagMatrix[4] * tagMatrix[15] - tagMatrix[2] * tagMatrix[7] * tagMatrix[12] - tagMatrix[3] * tagMatrix[4] * tagMatrix[14] + tagMatrix[3] * tagMatrix[6] * tagMatrix[12],
            +tagMatrix[0] * tagMatrix[6] * tagMatrix[11] - tagMatrix[0] * tagMatrix[7] * tagMatrix[10] - tagMatrix[2] * tagMatrix[4] * tagMatrix[11] + tagMatrix[2] * tagMatrix[7] * tagMatrix[8] + tagMatrix[3] * tagMatrix[4] * tagMatrix[10] - tagMatrix[3] * tagMatrix[6] * tagMatrix[8],

            +tagMatrix[4] * tagMatrix[9] * tagMatrix[15] - tagMatrix[4] * tagMatrix[11] * tagMatrix[13] - tagMatrix[5] * tagMatrix[8] * tagMatrix[15] + tagMatrix[5] * tagMatrix[11] * tagMatrix[12] + tagMatrix[7] * tagMatrix[8] * tagMatrix[13] - tagMatrix[7] * tagMatrix[9] * tagMatrix[12],
            -tagMatrix[0] * tagMatrix[9] * tagMatrix[15] + tagMatrix[0] * tagMatrix[11] * tagMatrix[13] + tagMatrix[1] * tagMatrix[8] * tagMatrix[15] - tagMatrix[1] * tagMatrix[11] * tagMatrix[12] - tagMatrix[3] * tagMatrix[8] * tagMatrix[13] + tagMatrix[3] * tagMatrix[9] * tagMatrix[12],

            +tagMatrix[0] * tagMatrix[5] * tagMatrix[15] - tagMatrix[0] * tagMatrix[7] * tagMatrix[13] - tagMatrix[1] * tagMatrix[4] * tagMatrix[15] + tagMatrix[1] * tagMatrix[7] * tagMatrix[12] + tagMatrix[3] * tagMatrix[4] * tagMatrix[13] - tagMatrix[3] * tagMatrix[5] * tagMatrix[12],
            -tagMatrix[0] * tagMatrix[5] * tagMatrix[11] + tagMatrix[0] * tagMatrix[7] * tagMatrix[9] + tagMatrix[1] * tagMatrix[4] * tagMatrix[11] - tagMatrix[1] * tagMatrix[7] * tagMatrix[8] - tagMatrix[3] * tagMatrix[4] * tagMatrix[9] + tagMatrix[3] * tagMatrix[5] * tagMatrix[8],

            -tagMatrix[4] * tagMatrix[9] * tagMatrix[14] + tagMatrix[4] * tagMatrix[10] * tagMatrix[13] + tagMatrix[5] * tagMatrix[8] * tagMatrix[14] - tagMatrix[5] * tagMatrix[10] * tagMatrix[12] - tagMatrix[6] * tagMatrix[8] * tagMatrix[13] + tagMatrix[6] * tagMatrix[9] * tagMatrix[12],
            +tagMatrix[0] * tagMatrix[9] * tagMatrix[14] - tagMatrix[0] * tagMatrix[10] * tagMatrix[13] - tagMatrix[1] * tagMatrix[8] * tagMatrix[14] + tagMatrix[1] * tagMatrix[10] * tagMatrix[12] + tagMatrix[2] * tagMatrix[8] * tagMatrix[13] - tagMatrix[2] * tagMatrix[9] * tagMatrix[12],

            -tagMatrix[0] * tagMatrix[5] * tagMatrix[14] + tagMatrix[0] * tagMatrix[6] * tagMatrix[13] + tagMatrix[1] * tagMatrix[4] * tagMatrix[14] - tagMatrix[1] * tagMatrix[6] * tagMatrix[12] - tagMatrix[2] * tagMatrix[4] * tagMatrix[13] + tagMatrix[2] * tagMatrix[5] * tagMatrix[12],
            +tagMatrix[0] * tagMatrix[5] * tagMatrix[10] - tagMatrix[0] * tagMatrix[6] * tagMatrix[9] - tagMatrix[1] * tagMatrix[4] * tagMatrix[10] + tagMatrix[1] * tagMatrix[6] * tagMatrix[8] + tagMatrix[2] * tagMatrix[4] * tagMatrix[9] - tagMatrix[2] * tagMatrix[5] * tagMatrix[8] };

    // det.
    double deta = tagMatrix[0] * ma[0] + tagMatrix[4] * ma[1] + tagMatrix[8] * ma[2] + tagMatrix[12] * ma[3];

    // inverse = (a*) / deta
    double esp = 0.000001;
    if (deta > esp || deta < -esp)
    {
        double deti = 1.0 / deta;
        resultMatrix[0] = ma[0] * deti;
        resultMatrix[1] = ma[1] * deti;
        resultMatrix[2] = ma[2] * deti;
        resultMatrix[3] = ma[3] * deti;
        resultMatrix[4] = ma[4] * deti;
        resultMatrix[5] = ma[5] * deti;
        resultMatrix[6] = ma[6] * deti;
        resultMatrix[7] = ma[7] * deti;
        resultMatrix[8] = ma[8] * deti;
        resultMatrix[9] = ma[9] * deti;
        resultMatrix[10] = ma[10] * deti;
        resultMatrix[11] = ma[11] * deti;
        resultMatrix[12] = ma[12] * deti;
        resultMatrix[13] = ma[13] * deti;
        resultMatrix[14] = ma[14] * deti;
        resultMatrix[15] = ma[15] * deti;

        return 1;
    }

    return 0;

}

__device__ void LLE2XYZ(double lon, double lat, double elev, double &x, double &y,double &z)
{
    lat = degToRad * lat;
    lon = degToRad * lon;
    double ER = earthRadius * 1000 + (double)elev;
    x = ER * cos(lat) * cos(lon);
    y = ER * cos(lat) * sin(lon);
    z = ER * sin(lat);
};

__device__ void XYZ2LLE(double x, double y,double z, double &lon,double &lat, double &elev)
{
    elev = (double)sqrt((double)(x*x+y*y+z*z))-earthRadius*1000;
    lon =  (double)(atan(y / x)) * (double)radToDeg;
    lat =  (double)(atan(z / (sqrt(x * x + y * y)))) * (double)radToDeg;
}

__device__ double getDistance(double lon1, double lat1, double lon2, double lat2)
{
    double radLat1 = lat1 * degToRad;
    double radLat2 = lat2 * degToRad;
    double a = radLat1 - radLat2;
    double b = lon1 * degToRad - lon2 * degToRad;
    double s = 2 * asin(sqrt(sin(a / 2) * sin(a / 2) * sin(a / 2) + cos(radLat1) * cos(radLat2) * sin(b / 2) * sin(b / 2)));
    s = s * earthRadius*1000;
    return s;
}

__device__ double getYaw(double lon1, double lat1, double lon2, double lat2)
{
    double result = 0.0;

    int ilat1 = (int)(0.50 + lat1 * 360000.0);
    int ilat2 = (int)(0.50 + lat2 * 360000.0);
    int ilon1 = (int)(0.50 + lon1 * 360000.0);
    int ilon2 = (int)(0.50 + lon2 * 360000.0);

    lat1 = double(lat1 * degToRad);
    lon1 = double(lon1 * degToRad);
    lat2 = double(lat2 * degToRad);
    lon2 = double(lon2 * degToRad);

    if ((ilat1 == ilat2) && (ilon1 == ilon2))
    {
        return result;
    }
    else if (ilon1 == ilon2)
    {
        if (ilat1 > ilat2)
        {
            result = 180.0;
        }
    }
    else
    {
        double c = acos(sin(lat2) * sin(lat1) + cos(lat2) * cos(lat1) * cos((lon2 - lon1)));
        double A = asin(cos(lat2) * sin((lon2 - lon1)) / sin(c));

        result = A * radToDeg;

        if ((ilat2 > ilat1) && (ilon2 > ilon1)) {}
        else if ((ilat2 < ilat1) && (ilon2 < ilon1)) { result = 180.0 - result; }
        else if ((ilat2 < ilat1) && (ilon2 > ilon1)) { result = 180.0 - result; }
        else if ((ilat2 > ilat1) && (ilon2 < ilon1)) { result += 360.0; }
    }

    if (result < 0) { result += 360.0; }
    if (result > 360) { result -= 360.0; }

    return result;
}

__device__ double calcPitch(double x0, double y0, double z0, double x1, double y1, double z1)
{
    double dv[3] = { x1 - x0, y1 - y0, z1 - z0 };
    double q = dv[0] * x0 + dv[1] * y0 + dv[2] * z0;
    if (q < 0)
        return -1;
    return (double)(q * q / (x0 * x0 + y0 * y0 + z0 * z0) / (dv[0] * dv[0] + dv[1] * dv[1] + dv[2] * dv[2]));
}

#pragma endregion